#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
from yahboomcar_msgs.msg import *
import time
import cv2
import numpy as np
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, Imu, JointState, LaserScan
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion
from cv_bridge import CvBridge
from std_msgs.msg import String

class RobotArm:
    def __init__(self):
        self.arm_pub = rospy.Publisher('/TargetAngle', ArmJoint, queue_size=100)
        self.JOINT_NAMES = ["joint1", "joint2", "joint3", "joint4", "joint5", "grip_joint"]
        self.joints_angle_deg = [90.0, 90.0, 90.0, 90.0, 90.0, 90.0]
        self.joints_angle_rad = [0.0, 0.6, -1.57, -0.785, -1.57, -1.38]  # Initial joint angles
        self.pi = math.pi

    def publish_arm(self):
        """Publish arm joint states"""
        # joint_state = JointState()  # Create ROS joint state message
        # joint_state.header.stamp = rospy.Time.now()
        # self.joints_angle_deg = [math.degrees(deg) for deg in self.joints_angle_rad]
        for idx, joint_angle in enumerate(self.joints_angle_deg):
            for _ in range(5):
                arm_joint = ArmJoint()
                arm_joint.id = idx + 1
                arm_joint.run_time = 10
                arm_joint.angle = joint_angle # unit: radians
                self.arm_pub.publish(arm_joint)
                rospy.sleep(0.15)


if __name__ == "__main__":
    rospy.init_node('arm_test', anonymous=True)
    rate = rospy.Rate(10)  # 1Hz
    try:
        arm = RobotArm()
        arm.publish_arm()
        arm.joints_angle_deg = [90.0, 90.0, 120.0, 30.0, 0.0, 0.0]
        arm.publish_arm()
    except rospy.ROSInterruptException:
        pass
    # rospy.spin()